function y = R6ki(x) 
% 
% Position so1ution of a RRR bar group 
% 
% so1ving acce1eration of 6R three degree bar group 
% 
% C.Opyright 2002 Qu Xiuquan 
% 
% Input parameters 
% 
%x(1)=ri 
% x(2) = rj 
% x(3) = rk 
% x(4) = e 
% x(5) = f 
% x(6) = g 
% x(7) = theta - i 
% x(8) = theta
% x(9) = theta - k 
% x(1O) = theta
% x(11) = theta - f 
% x(12) = theta - g 
% x(13) = dtheta - i 
% x(14) = dtheta - j 
% x(15) = dtheta - k 
% x(16) = dtheta - e 
% x(17) = dthe -f
% x(18) = dtheta - g 
% x(19) = Re[ddB] 
% x(20) = Im[ddB] 
% x(21) = Re[ddC] 
% x(22) = Im[ddC] 
% x(23) = Re[ddD] 
% x(24) = Im[ddD) 
% 
% Output parameters
% 
% y(1) = ddtheta
% y(2) = ddtheta - j 
% y(3) = ddtheta - k 
% y(4) = ddtheta
% y(5) = ddtheta - f 
% y(6) = ddtheta - g 
% y(7) = Re[ddE] 
% y(8) = Im[ddE] 
% y(9) = Re[ddF] 
% y(IO) = Im[ddF] 
% y(11) = Re[ddG] 
% y(12) = Im[ddG) 
% 
a = [-x(1)*cos(x(7)+pi/2) x(2)*cos(x(8)+pi/2) 0 0 0 -x(6)*cos(x(12)+pi/2); 
     -x(1)*sin(x(7)+pi/2) x(2)*sin(x(8)+pi/2) 0 0 0 -x(6)*sin(x(12)+pi/2); 
     0 -x(2)*cos(x(8)+pi/2) x(3)*cos(x(9)+pi/2) -x(4)*cos(x(10)+pi/2) 0 0;
0 -x(2)*sin(x(8)+pi/2) x(3)*sin(x(9)+pi/2) -x(4)*sin(x(10)+pi/2) 0 0;
x(1)*cos(x(7)+pi/2) 0 -x(3)*cos(x(9)+pi/2) 0 -x(5)*cos(x(11)+pi/2) 0;
x(1)*sin(x(7)+pi/2) 0 -x(3)*sin(x(9)+pi/2) 0 -x(5)*sin(x(11)+pi/2) 0];
disp(a)
c = - [-x(1)*cos(x(7)+pi) x(2)*cos(x(8)+pi) 0 0 0 -x(6)*cos(x(12)+pi);
-x(1)*sin(x(7)+pi) x(2)*sin(x(8)+pi) 0 0 0 -x(6)*sin(x(12)+pi);
0 -x(2)*cos(x(8)+pi) x(3)*cos(x(9)+pi) -x(4)*cos(x(10)+pi) 0 0; 
0 -x(2)*sin(x(8)+pi) x(3)*sin(x(9)+pi) -x(4)*sin(x(10)+pi) 0 0; 
x(1)*cos(x(7)+pi) 0 -x(3)*cos(x(9)+pi) 0 -x(5)*cos(x(11)+pi) 0; 
x(1)*sin(x(7)+pi) 0 -x(3)*sin(x(9)+pi) 0 -x(5)*sin(x(11)+pi) 0]; 
b1 = c*[x(13)^2;x(14)^2;x(15)^2;x(16)^2;x(17)^2;x(18)^2]; 
disp(b1)
b2 = [x(19)-x(21);x(20)-x(22);x(21)-x(23);x(22)-x(24);x(23)-x(19);x(24)-x(20)]; 
disp(b2)
b =b1+ b2; 
ddtheta = inv(a) * b; 
disp(ddtheta)
y(1) = ddtheta(1) ; 
y(2) = ddtheta(2); 
y(3) = ddtheta(3); 
y(4) = ddtheta(4); 
y(5) = ddtheta(5); 
y(6) = ddtheta(6); 
y(7) = x(19) + x(1) * ddtheta(1) * cos(x(7) + pi/2) + x(1) * x(13)^2 * cos(x(7) + pi); 
y(8) = x(20) + x(1) * ddtheta(1) * sin(x(7) + pi/2) + x(1) * x(13)^2* sin(x(7) + pi); 
y(9) = x(21) + x(2) * ddtheta(2) * cos(x(8) + pi/2) + x(2) * x(14)^2 * cos(x(8) +pi); 
y(10) = x(22) + x(2) * ddtheta(2) * sin(x(8) + pi/2) + x(2) * x(14)^2 * sin(x(8) +pi); 
y(11) = x(23) + x(3) * ddtheta(3) * cos(x(9) + pi/2) + x(3) * x(15)^2* cos(x(9) + pi); 
y(12) = x(24) + x(3) * ddtheta(3) * sin(x(9) + pi/2) + x(3) * x(15)^2 * sin(x(9) +pi);